Radar Measurements

H versus h(x)

The H matrix from the lidar lesson and h(x) equations from the radar lesson are actually accomplishing the same thing; they are both needed to solve y = z - Hx' in the update step.

But for radar, there is no H matrix that will map the state vector x into polar coordinates; instead, you need to calculate the mapping manually to convert from cartesian coordinates to polar coordinates.

Here is the h function that specifies how the predicted position and speed get mapped to the polar coordinates of range, bearing and range rate.

h(x')= \begin{pmatrix} \rho\\ \phi\\ \dot{\rho} \end{pmatrix} = \begin{pmatrix} \sqrt{ p{'}_x^2 + p{'}_y^2 }\\ \arctan(p_y' / p_x')\\ \frac{p_x' v_x' + p_y' v_y'}{\sqrt{p{'}_x^2 + p{'}_{y}^2}} \end{pmatrix}

Hence for radar y = z - Hx' becomes y = z - h(x').

Definition of Radar Variables

The range, (\rho), is the distance to the pedestrian. The range is basically the magnitude of the position vector \rho which can be defined as \rho = sqrt(p_x^2 + p_y^2).
\varphi = atan(p_y / p_x). Note that \varphi is referenced counter-clockwise from the x-axis, so \varphi from the video clip above in that situation would actually be negative.
The range rate, \dot{\rho}, is the projection of the velocity, v, onto the line, L.

Deriving the Radar Measurement Function

The measurement function is composed of three components that show how the predicted state, x' = (p_x', p_y', v_x', v_y')^T, is mapped into the measurement space, z = (\rho, \varphi, \dot{\rho})^T:

The range, \rho, is the distance to the pedestrian which can be defined as:

\rho = \sqrt[]{p_x^2 + p_y^2}

\varphi is the angle between \rho and the x direction and can be defined as:

\varphi = \arctan(p_y/p_x)

There are two ways to do the range rate \dot{\rho(t)} derivation:

Generally we can explicitly describe the range, \rho, as a function of time:

\rho(t) = \sqrt{p_x(t)^2 + p_y(t)^2}

The range rate, \dot{\rho(t)}, is defined as time rate of change of the range, \rho, and it can be described as the time derivative of \rho:

\dot{\rho} = \frac{\partial \rho(t)}{\partial t} = \frac{ \partial}{\partial t}\sqrt{p_x(t)^2 + p_y(t)^2} = \frac{1}{2 \sqrt{p_x(t)^2 + p_y(t)^2}} (\frac{ \partial}{\partial t}p_x(t)^2 + \frac{ \partial}{\partial t}p_y(t)^2)

=\frac{1}{2 \sqrt{p_x(t)^2 + p_y(t)^2}} (2 p_x(t) \frac{ \partial}{\partial t} p_x(t) + 2 p_y(t) \frac{ \partial}{\partial t} p_y(t))

\frac{ \partial}{\partial t} p_x(t) is nothing else than v_x(t), similarly \frac{ \partial}{\partial t} p_y(t) is v_y(t). So we have:

\dot{\rho} = \frac{\partial \rho(t)}{\partial t} = \frac{1}{2 \sqrt{p_x(t)^2 + p_y(t)^2}} (2 p_x(t) v_x(t) + 2 p_y(t) v_y(t)) = \frac{2( p_x(t) v_x(t) + p_y(t) v_y(t))}{2 \sqrt{p_x(t)^2 + p_y(t)^2}}

=\frac{p_x(t) v_x(t) + p_y(t) v_y(t)}{ \sqrt{p_x(t)^2 + p_y(t)^2}}

For simplicity we just use the following notation:

\dot{\rho} = \frac{p_x v_x + p_y v_y}{ \sqrt{p_x^2 + p_y^2}}

The range rate, \dot{\rho}, can be seen as a scalar projection of the velocity vector, \vec{v}, onto \vec{\rho}. Both \vec{\rho} and \vec{v} are 2D vectors defined as:

\vec{\rho}= \begin{pmatrix} p_x\\ p_y \end{pmatrix}, \space \vec{v}=\begin{pmatrix} v_x\\ v_y \end{pmatrix}

The scalar projection of the velocity vector \vec{v} onto \vec{\rho} is defined as:

\dot{\rho}= \frac{\vec{v} \vec{\rho}}{\lvert \vec{\rho} \rvert} = \frac{ \begin{pmatrix} v_x & v_y \end{pmatrix} \begin{pmatrix} p_x\\ p_y \end{pmatrix} }{ \sqrt{p_x^2 + p_y^2} } = \frac{p_x v_x + p_y v_y}{ \sqrt{p_x^2 + p_y^2}}

where \lvert \vec{\rho} \rvert is the length of \vec{\rho}. In our case it is actually the range, so \rho = \lvert \vec{\rho} \rvert.

The Next Quiz

\begin{pmatrix} \rho\\ \phi\\ \dot{\rho} \end{pmatrix} \leftarrow h(x) \begin{pmatrix} p_x'\\ p_y'\\ v_x'\\ v_y' \end{pmatrix}

h is a nonlinear function. In the next quiz I would like to check your intuition about what that means.

Next Concept